#include "stdafx.h"
#include "Agent.hpp"
#include "RandGenFast.hpp"
#include "Simulation.hpp"
#include "SteeringBehaviors.hpp"
#include "PropertyManager.hpp"


//////////////////////////////////////////////////////////////////////////

Agent::Agent(const vec3& pos)
	: _position(pos), _velocity(0.0f), _tagged(false), _headingSmoother(10, vec3::ZERO),
	_sceneNode(0)
{
	_heading = vec3(rnd.RandFloat(-1.0f, 1.0f), rnd.RandFloat(-1.0f, 1.0f)/4, rnd.RandFloat(-1.0f, 1.0f));
	_heading.normalise();
	_headingSmoothed = _headingSmoother.update(_heading);
}

//////////////////////////////////////////////////////////////////////////

void Agent::update(float dt)
{
	vec3 netForce = calcSteeringForce(dt);
	vec3 totalForce = forwardComponent(netForce);
	totalForce += -_heading * _velocity.length() * PM.damping->getRealValue();

	vec3 accel = totalForce / getMass();
	_velocity += accel * dt;
	if (_velocity.squaredLength() > getMaxVelocity()*getMaxVelocity())
		_velocity = _velocity.normalisedCopy() * getMaxVelocity();
	_position += _velocity * dt;

	if (_velocity.squaredLength() < PM.zeroVelocity->getRealValue()*PM.zeroVelocity->getRealValue())
		_velocity = vec3::ZERO;

	vec3 side = sideComponent(netForce);
	quat rotQuat = _heading.getRotationTo(side.normalisedCopy());
	vec3 axis;
	Ogre::Degree angle;
	rotQuat.ToAngleAxis(angle, axis);
	float rotAngle = angle.valueDegrees();
	if (fabsf(rotAngle) > getTurnRate()) 
		rotAngle = getTurnRate() * (rotAngle > 0.0f? 1 : -1);
	rotQuat = quat(Ogre::Degree(rotAngle), axis);
	_heading  = rotQuat * _heading;

	if (_velocity != vec3::ZERO)
		_velocity = _heading * _velocity.length();

	_headingSmoothed = _headingSmoother.update(_heading);
	_headingSmoothed.normalise();
}

//////////////////////////////////////////////////////////////////////////

vec3 Agent::forwardComponent(const vec3& v) const
{
	return _heading * v.dotProduct(_heading);
}
vec3 Agent::sideComponent(const vec3& v) const
{
	return v - forwardComponent(v);
}

//////////////////////////////////////////////////////////////////////////
